/*
*** Modified 09/19/2011 to return blips per second
 
 ***  countBlips - measures the time taken to move some 
 number of tachometer "blips"
 
 Modified for two motors
 
 Modified for two channels from each motor 12/13/2011
 */

int ledPin11 = 11;                      // Motor1 LED (pin 11)
int ledPin12 = 12;                      // Motor2 LED (pin 12) 
int ChBpin1 = 4;                        // Channel B encoder data for motor 1
int ChBpin2 = 5;                        // ditto motor 2

volatile unsigned long blipCounterA1 = 0;  // Counter for number of blips seen on Tach1 (from motor1)
volatile unsigned long blipCounterA2 = 0;  // Counter for number of blips seen on Tach2 (from motor2)
//volatile unsigned long blipCounterB1 = 0;  // Counter for number of blips seen on Tach1 (from motor1) on channel B
//volatile unsigned long blipCounterB2 = 0;  // Counter for number of blips seen on Tach2 (from motor2) on channel B (green wire vs blue wire)

int flashLEDs = HIGH;

unsigned long current_time = 0;    // Variable to store current time in microseconds
unsigned long last_time    = 0;       // Variable to store last time in microseconds

unsigned long blips_from_A1 = 0;    // Var to store blipCounterA1's value every time_increment microseconds
unsigned long blips_from_A2 = 0;    // Var to store blipCounterA2's value every time_increment microseconds
int ChB1state = HIGH;
int ChB2state = HIGH;                      // Vars to store states of B channels (high or low)
int ChB1stateCapture;
int ChB2stateCapture;                // Vars to store captured value of B channels

//unsigned long blips_from_B1 = 0;    // Var to store blipCounterB1's value every time_increment microseconds
//unsigned long blips_from_B2 = 0;    // Var to store blipCounterB2's value every time_increment microseconds

String totalBlips;      // String to print to serial line

unsigned long time_increment = 500000;               // set time to increment by (microseconds) 
// 500 milliseconds is .5 seconds and 500 000 microseconds
//char outputBuffer[256];

void setup()
{
  pinMode(ledPin11, OUTPUT);              // Configure motor1 LED pin
  pinMode(ledPin12, OUTPUT);              // Configure motor2 LED pin
  
  pinMode(ChBpin1, INPUT);                // Configure motor 1's channel B tach data to be input
  pinMode(ChBpin2, INPUT);                // ditto motor 2's channel B
  digitalWrite(2, HIGH);                 // Set pullup resistor on external interrupt pin 2 (data from Tach1)
  digitalWrite(3, HIGH);                 // Set pullup resistor on external interrupt pin 3 (data from Tach2)
  //digitalWrite(4, HIGH);                 // Set pullup resistor on  pin 4 (data from Tach1) channel B
  //digitalWrite(5, HIGH);                 // Set pullup resistor on  pin 5 (data from Tach2) channel B


  attachInterrupt(0, countBlipsA1, RISING);  // Attach Interrupt Routine to External Interrupt 0, on pin 2, to trigger an interrupt on signal's rising edge (Tach1)
  attachInterrupt(1, countBlipsA2, RISING);  // Attach Interrupt Routine to External Interrupt 1, on pin 3, to trigger an interrupt on signal's rising edge (Tach2)
  //attachInterrupt(2, countBlipsB1, RISING);  // Attach Interrupt Routine to External Interrupt 2, on pin 4, to trigger an interrupt on signal's rising edge (Tach1)
 // attachInterrupt(3, countBlipsB2, RISING);  // Attach Interrupt Routine to External Interrupt 3, on pin 5, to trigger an interrupt on signal's rising edge (Tach2)
  // actually no such thing as attachInterrupt(2) or (3) for the Deumilanove
  Serial.begin(9600);                    // Start serial connection

}

void loop()
{ 
  /*ChB1state = digitalRead(ChBpin1);
  Serial.println((String)ChB1state);*/
  current_time = micros();    // Snag current time in microseconds

    if( (current_time - last_time) >= time_increment ){
      blips_from_A1 = blipCounterA1;
      ChB1stateCapture = ChB1state;
      blips_from_A2 = blipCounterA2;
      ChB2stateCapture = ChB2state;
       // snag current num counts and states of B channels
       
      blipCounterA1 = 0;
      blipCounterA2 = 0;   // reset counters   
   
// totalBlips has form m1blips 1/0 m2blips 1/0 time

      //Serial.println((String)blips_from_A1 + " " + (String)ChB1stateCapture);
      totalBlips = (String)blips_from_A1; 
      if(ChB1stateCapture == LOW){ // if B was low when A went high, A leads B and wheel's turning forward 
        totalBlips.concat(" 0 "); 
      }
      if(ChB1stateCapture == HIGH){ // otherwise B was high when A went high and B leads A, so wheel's turning backward
        totalBlips.concat(" 1 ");
      }
     
      // Motor1's and Motor2's 1/0 convention have to be reversed because they are mounted oppositely. 
     
      totalBlips.concat((String)blips_from_A2);
    
      if(ChB2stateCapture == LOW){ 
        totalBlips.concat(" 1 "); 
      }
      if(ChB2stateCapture == HIGH){
        totalBlips.concat(" 0 "); 
      }
    
      totalBlips.concat((String)(current_time - last_time));

      Serial.println(totalBlips);
      // Output is in the form 'xxx 1/0 yyy 1/0 zzzzzz'
      // where x is the number of blips from tach1, y is the number of
      // blips from tach 2, and z is the time increment in microseconds
      // during which these measurements were taking. Blips per second for each motor
      // the 1/0 after xxx and yyy indicate forwardness or backwardness of motor movement

      digitalWrite(ledPin11, flashLEDs);   // LEDs toggle state wgen time_increment was reached
      digitalWrite(ledPin12, flashLEDs);   // these aren't working for some reason, although
      // the onboard led (13?) flashes sometimes
      flashLEDs = ! flashLEDs;  

      last_time = current_time;
      // Oscillator is 16 MHz
      // So ever 1/16e6 seconds, a command is executed.
      // ie, every 6.25 * 10^-8 = .0625 microseconds
    } 
}

void countBlipsA1()          // Routine to run upon recieving interrupt on pin 2 (from Tach1)
{
  blipCounterA1++;           // Just increment the blip counter1 
  ChB1state = digitalRead(ChBpin1); // and capture state of channel B
 
}

void countBlipsA2()          // Routine to run upon recieving interrupt on pin 3 (from Tach2)
{
  blipCounterA2++;           // Just increment the blip counter 
  ChB2state = digitalRead(ChBpin2); // and capture state of channel B
}


